﻿#include "uim.h"
#include "LoadSimCanFunc.h"

UIM::UIM(const QString dllName)
{
    ret = LoadSimCanFunc(dllName);
}

UIM::~UIM()
{
    FreeSimCanFunc();
}

void UIM::Init(void)
{
    canRightNodeId = 0;
    canFocusNodeId = 0;

    isCanRightMove = false;

    // isCanFocusS1 = false;
    // isCanFocusS2 = false;
    isCanFocusMove = 0;
    isCanFocusMoveType = false;

    isCanRightEnable = false;
    isCanFocusEnable = false;

    memset(&canRightPara, 0, sizeof(UIM_PARA));
    memset(&canFocusPara, 0, sizeof(UIM_PARA));

    setCanFocusRevStep(0);

    canRightOff();
    canFocusOff();

    // canFocusResetMark = 0;
    canGatewayCount = 0;
    canCurGatewayIndex = -1;
    isCanGateway = false;
}

void UIM::canSearch(void)
{
    if(canGatewayCount > 0 && canCurGatewayIndex > 0){
        return;
    }
    unsigned int uimType=0;
    DEV_INFO_OBJ canDevInfoObj[5];
    int  dwGateWayLen = -1;
    if(conf->uimVar.UIMUsbPort){
        retInfo("SearchGateway UIDEV_USBCAN");
        uimType |= UIDEV_USBCAN;
    }
    if(conf->uimVar.UIMComPort){
        // retInfo(QString("can com %0 %1").arg(conf->uimVar.UIMComPos).arg(conf->uimVar.UIMComRate));
        // SetQuickScan(conf->uimVar.UIMComPos,conf->uimVar.UIMComRate,1);
        retInfo(QString("SearchGateway UIDEV_RS232CAN "));
        uimType |= UIDEV_RS232CAN;
    }
    dwGateWayLen = SearchGateway(uimType, NULL, canDevInfoObj, 5);
    if (-1 == dwGateWayLen)
    {
        retInfo("Query gateway failed!");
        return;
    }
    canGatewayCount = dwGateWayLen;
    retInfo(QString("Query gateway %0").arg(canGatewayCount));
    for (int  i = 0; i < canGatewayCount; i++)
    {
        canGatewayNameList.append(QString("%0->%1")
                                  .arg(canDevInfoObj[i].pszDevName)
                                  .arg(canDevInfoObj[i].dwDevIndex));
        canGatewayIndex[i]=canDevInfoObj[i].dwDevIndex;
        retInfo(QString("gateway %0->%1:%2")
                .arg(i)
                .arg(canDevInfoObj[i].pszDevName)
                .arg(canDevInfoObj[i].dwDevIndex));
    }
    canCurGatewayIndex = canGatewayIndex[0];
    isCanGateway = false;
}

bool UIM::isOpen(void){
    return isCanGateway;
}

//void UIM::setOpen(bool en){
//    isCanGateway = en;
//}

int UIM::getIndex(void){
    return canCurGatewayIndex;
}

QStringList UIM::getList(void){
    return canGatewayNameList;
}

int UIM::getFocusId(void){
    return canFocusNodeId;
}
int UIM::getFocusPos(void){
    return canGetPos(canFocusNodeId);
}
int UIM::getFocusMoveStep(void){
    if(isCanFocusMoveType){
        return canFocusPos;
    }else{
        return canFocusStep;
    }
}
bool UIM::isFocusId(int id){
    if(id == 0){
        return (canFocusNodeId>0);
    }
    return id == canFocusNodeId;
}
bool UIM::isFocus(void){
    return isCanFocusEnable;
}
// bool UIM::getCanFocus1(void){
//     return isCanFocusS1;
// }
// void UIM::setCanFocus1(bool in){
//     isCanFocusS1 = in;
// }
// bool UIM::getCanFocus2(void){
//     return isCanFocusS2;
// }
// void UIM::setCanFocus2(bool in){
//     isCanFocusS2 = in;
// }
int UIM::getCanFocusMoveDirection(void){
    return isCanFocusMove;
}
void UIM::setCanFocusMoveDirection(int mv){
    isCanFocusMove = mv;
}
int UIM::getCanFocusRevStep(void){
    return canFocusRevStep;
}
void UIM::setCanFocusRevStep(int step){
    canFocusRevStep = step;
}


// int UIM::getEncodeId(void){
//     return canEncodeNodeId;
// }

// int UIM::getEncode(bool Qec){
//     if(canEncodeNodeId>0){
//         if(Qec){
//             return canGetQec(canEncodeNodeId);
//         }else{
//             return canGetPos(canEncodeNodeId);
//         }
//     }
//     return 0;
// }

int UIM::getRightId(void){
    return canRightNodeId;
}
int UIM::getRightPos(void){
    return canGetPos(canRightNodeId);
}
bool UIM::isRightId(int id){
    if(id == 0){
        return (canRightNodeId>0);
    }
    return id == canRightNodeId;
}
bool UIM::isRight(void){
    return isCanRightEnable;
}
bool UIM::isRightMove(void){
    return isCanRightMove;
}

int UIM::compare(const void *first, const void *second)
{
    return *(unsigned int  *)first - *(unsigned int  *)second;
}

bool UIM::getPos(void){
    int pos;
    pos = canGetPos(getRightId());
    if(pos<0){
        return false;
    }
    if(curPosCounter==0 && curPosTimer == 0){
        curPosCounter =pos;
        curPosTimer = QDateTime::currentMSecsSinceEpoch();
        return false;
    }
    curFramePosTimer = QDateTime::currentMSecsSinceEpoch() - curPosTimer;
    // retInfo(QString("getPos %0 %1").arg(pos).arg(curPosCounter));
    curFramePosCounter = abs(pos - curPosCounter);
    curPosCounter = pos;
    curPosTimer = QDateTime::currentMSecsSinceEpoch();
    return true;
}
bool UIM::isPosOver(int timer){
    if(curPosCounter==0){
        getPos();
    }
    return (QDateTime::currentMSecsSinceEpoch()-curPosTimer)>timer;
}

bool UIM::canOpenGateWay(void)
{
    //搜索
    canSearch();
    //关闭
    canCloseGateWay();

    MDL_INFO_OBJ canMDLInfoObj[120];
    unsigned int canBtr;
    unsigned int canNodeId[120];
    memset(canNodeId, 0, sizeof(unsigned int )*120);
    memset(canMDLInfoObj, 0, sizeof(MDL_INFO_OBJ)*120);

    canNodeLen = OpenGateway(canCurGatewayIndex, canNodeId, 120, &canBtr);
    if (-1 == canNodeLen)
    {
        retInfo(QString("Open gateway [%0]failed!").arg(canCurGatewayIndex));
        return false;
    }
    QString msg="";
    qsort(canNodeId, canNodeLen, sizeof(unsigned int), compare);
    for(int i  = 0; i < canNodeLen;++i)
    {
        GetMDL(canCurGatewayIndex, canNodeId[i], &(canMDLInfoObj[i]));
        msg +=QString("[%0->%1:%2]").arg(i).arg(canNodeId[i]).arg(canMDLInfoObj[i].szModelName);
    }
    retInfo(QString("open nodeLen=%0->%1").arg(canNodeLen).arg(msg));
    for(int i  = 0; i < canNodeLen;++i)
    {
        //胶片-八个电机赋值
        if(conf->uimVar.UIMNodeIdFocus == (int)canNodeId[i]){
            canFocusNodeId = canNodeId[i];
        }
        if(conf->uimVar.UIMNodeIdRight == (int)canNodeId[i]){
            canRightNodeId = canNodeId[i];
        }
    }
    isCanGateway = true;
    return true;
}

void UIM::canCloseGateWay(void)
{
    if(isCanGateway){
        if (-1 == CloseGateway(canCurGatewayIndex))
        {
            retInfo(QString("Close gateway [%0] failed!").arg(canCurGatewayIndex));
        }
        Init();
    }
}

//使能
bool UIM::canEna(bool &en, int id, const QString msg, UIM_PARA *para){
    if(en){
        return true;
    }
    if(id <= 0){
        return false;
    }

    BASIC_ACK_OBJ objBasicAck;
    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
    if (-1 == UimENA(canCurGatewayIndex, id, 1, &objBasicAck))
    {
        retInfo(QString("can %0 Ena fail!").arg(msg));
        return false;
    }
    en = true;
    canSetSPD(id,0);
    canSetSTP(id,0);
    if(para != nullptr){
        para->bENA = objBasicAck.bENA;          //电机使能状态
        para->bDIR = objBasicAck.bDIR;          //电机方向
        para->bACR = objBasicAck.bACR;          //电流减半
        para->uiMCS = objBasicAck.uiMCS;        //电机细分
        para->uiCUR = objBasicAck.uiCUR;        //电流
        para->uiSPD = objBasicAck.uiSPD;        //当前速度
        para->uiSTP = objBasicAck.uiSTP;        //当前步长
        retInfo(QString("%0 canEna para: %1,%2,%3,%4,%5,%6,%7").arg(msg)
                    .arg(para->bENA).arg(para->bDIR).arg(para->bACR).arg(para->uiMCS).arg(para->uiCUR).arg(para->uiSPD).arg(para->uiSTP));
    }
    return true;
}

//关闭
bool UIM::canOff(bool &en, int id, const QString msg, UIM_PARA *para){
    if (!en){
        return true;
    }
    if(id <= 0){
        return false;
    }
    BASIC_ACK_OBJ objBasicAck;
    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
    if (-1 == UimOFF(canCurGatewayIndex, id, 1, &objBasicAck))
    {
        retInfo(QString("can %0 Off fail!").arg(msg));
        return false;
    }
    en = false;
    if(para != nullptr){
        para->bENA = objBasicAck.bENA;          //电机使能状态
        para->bDIR = objBasicAck.bDIR;          //电机方向
        para->bACR = objBasicAck.bACR;          //电流减半
        para->uiMCS = objBasicAck.uiMCS;        //电机细分
        para->uiCUR = objBasicAck.uiCUR;        //电流
        para->uiSPD = objBasicAck.uiSPD;        //当前速度
        para->uiSTP = objBasicAck.uiSTP;        //当前步长
        retInfo(QString("%0 canOff para: %1,%2,%3,%4,%5,%6,%7").arg(msg)
                    .arg(para->bENA).arg(para->bDIR).arg(para->bACR).arg(para->uiMCS).arg(para->uiCUR).arg(para->uiSPD).arg(para->uiSTP));
    }
    return true;
}

UIM_PARA *UIM::getRightPara(void){
    return &canRightPara;
}
UIM_PARA *UIM::getFocusPara(void){
    return &canFocusPara;
}

//调焦电机
void UIM::canFocusEna(bool en){
    if(en){
        isCanFocusEnable = false;
    }
    canEna(isCanFocusEnable,canFocusNodeId,"focus ",&canFocusPara);
}

void UIM::canFocusOff(bool en){
    if(en){
        isCanFocusEnable = true;
    }
    setCanFocusMoveDirection(0);
    canOff(isCanFocusEnable,canFocusNodeId,"focus ",&canFocusPara);
}

//右电机
void UIM::canRightEna(bool en){
    if(en){
        isCanRightEnable = false;
    }
    canEna(isCanRightEnable,canRightNodeId,"right ",&canRightPara);
}

void UIM::canRightOff(bool en){
    if(en){
        isCanRightEnable = true;
    }
    canOff(isCanRightEnable,canRightNodeId,"right ",&canRightPara);
    isCanRightMove = false;
}

void UIM::canSetSPD(int index,int spd)
{
    if (-1 == SetSPD(canCurGatewayIndex, index, spd, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Set SPD=%1 fail!").arg(index).arg(spd));
        return;
    }
    if(index==canRightNodeId){
        isCanRightMove = (spd>0);
    }
    if(isCanRightMove){
        curPosCounter = 0;
        curPosTimer = 0;
    }
}

//相对位置
void UIM::canSetSTP(int index,int stp)
{
    if (-1 == SetSTP(canCurGatewayIndex, index, stp, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] STP=%1 fail!").arg(index).arg(stp));
        return;
    }
    if(index==canFocusNodeId){
        canFocusStep = stp;
        setCanFocusMoveDirection(getIntToMark(stp));
        isCanFocusMoveType = false;
        canFocusPos = 0;
        return;
    }
}

void UIM::canStop(int index){
    canSetSTP(index,0);
}

void UIM::canSetOrg(int index)
{
    if (-1 == SetORG(canCurGatewayIndex, index, 0, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] ORG fail!").arg(index));
        return;
    }
    canGetPos(index);
}

int UIM::canGetPos(int index)
{
    int value=-1;
    if (-1 == GetPOS(canCurGatewayIndex, index, &value))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Get POS fail!").arg(index));
        return value;
    }
    return value;
}

//绝对位置
void UIM::canSetPOS(int index,int pos)
{
    if (-1 == SetPOS(canCurGatewayIndex, index, pos, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] POS=%1 fail!").arg(index).arg(pos));
        return;
    }
    if(index==canFocusNodeId){
        canFocusPos = pos;
        setCanFocusMoveDirection(getIntToMark(pos));
        isCanFocusMoveType = true;
        canFocusStep = 0;
        return;
    }
}

void UIM::canSetQec(int index)
{
    int value=0;
    if (-1 == SetQEC(canCurGatewayIndex, index, value, false, NULL))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Set QEC fail!").arg(index));
        return;
    }
    retInfo(QString::fromLocal8Bit("can[%0] Set QEC success!").arg(index));
}

int UIM::canGetQec(int index)
{
    int value=0;
    if (-1 == GetQEC(canCurGatewayIndex, index, &value))
    {
        retInfo(QString::fromLocal8Bit("can[%0] Get QEC fail!").arg(index));
        return value;
    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get QEC %1 success!").arg(index).arg(value));
    return value;
}


//int UIM::canGetMcs(int index)
//{
//    int value=0;
//    BASIC_ACK_OBJ objBasicAck;
//    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
//    if (-1 == GetMCS(canCurGatewayIndex, index, &objBasicAck))
//    {
//        retInfo(QString::fromLocal8Bit("can[%0] Get MCS fail!").arg(index));
//        return value;
//    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get MCS %1 success!").arg(index).arg(value));
//    return value;
//}
//int UIM::canGetCur(int index)
//{
//    int value=0;
//    BASIC_ACK_OBJ objBasicAck;
//    memset(&objBasicAck, 0, sizeof(BASIC_ACK_OBJ));
//    if (-1 == GetCUR(canCurGatewayIndex, index, &objBasicAck))
//    {
//        retInfo(QString::fromLocal8Bit("can[%0] Get CUR fail!").arg(index));
//        return value;
//    }
//    retInfo(QString::fromLocal8Bit("can[%0] Get CUR %1 success!").arg(index).arg(value));
//    return value;
//}


//int UIM::canSetMcs(int index, int mcs)
//{
//    int value=0;
//    return value;
//}
//int UIM::canSetCur(int index, int cur)
//{
//    int value=0;
//    return value;
//}

bool UIM::isCanSet(const int node){
    if(node == getFocusId()){
        return (getFocusPos() != conf->uimVar.UIMCanStepFocusLocate);
    }
    return false;

}
